#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <linux/videodev2.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <iostream>
#include <cstring>


class UVCCameraNode {
private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Publisher image_pub_;
    int fd_;  // 相机设备文件描述符

    // 设备与发布配置
    std::string device_path_;
    std::string frame_id_;
    int publish_rate_;

    // 图像参数
    int width_;
    int height_;
    int fps_;

    // 相机控制参数（与v4l2-ctl查询结果匹配）
    int brightness_;        // 0-10
    int contrast_;          // 0-20
    int saturation_;        // 0-20
    int sharpness_;         // 0-20
    int gain_;              // 0-255
    int backlight_comp_;    // 0-64
    bool wb_auto_;          // 自动白平衡
    int wb_temp_;           // 色温档位1-5
    bool focus_auto_;       // 自动对焦
    int focus_abs_;         // 手动对焦0-1023

public:
    UVCCameraNode() : nh_("~"), it_(nh_) {
        // 从参数服务器读取配置（默认值适配你的相机）
        nh_.param("device_path", device_path_, std::string("/dev/video0"));
        nh_.param("frame_id", frame_id_, std::string("camera_link"));
        nh_.param("publish_rate", publish_rate_, 10);  // 高分辨率建议降低发布频率

        // 图像分辨率与帧率（MJPG支持最高4608x3456@30fps）
        nh_.param("width", width_, 4608);
        nh_.param("height", height_, 3456);
        nh_.param("fps", fps_, 30);

        // 相机控制参数
        nh_.param("brightness", brightness_, 5);
        nh_.param("contrast", contrast_, 10);
        nh_.param("saturation", saturation_, 10);
        nh_.param("sharpness", sharpness_, 0);
        nh_.param("gain", gain_, 0);
        nh_.param("backlight_compensation", backlight_comp_, 0);
        nh_.param("white_balance_auto", wb_auto_, true);
        nh_.param("white_balance_temperature", wb_temp_, 4);
        nh_.param("focus_automatic_continuous", focus_auto_, true);
        nh_.param("focus_absolute", focus_abs_, 512);

        // 打开相机设备（V4L2控制用）
        fd_ = open(device_path_.c_str(), O_RDWR);
        if (fd_ < 0) {
            ROS_FATAL("无法打开相机设备: %s（错误：%s）", 
                      device_path_.c_str(), strerror(errno));
            ros::shutdown();
            return;
        }

        // 初始化相机控制参数（对焦、白平衡等）
        initCameraControls();

        // 初始化图像发布器
        image_pub_ = it_.advertise("/" + frame_id_ + "/image_raw", 1);
        ROS_INFO("相机节点启动成功，设备: %s", device_path_.c_str());
    }

    ~UVCCameraNode() {
        if (fd_ >= 0) close(fd_);
        ROS_INFO("相机节点关闭");
    }

    // 设置V4L2控制参数（直接使用相机参数ID）
    bool setV4L2Control(unsigned int cid, int value, const std::string& name) {
        struct v4l2_control ctrl;
        ctrl.id = cid;
        ctrl.value = value;
        if (ioctl(fd_, VIDIOC_S_CTRL, &ctrl) == 0) {
            ROS_INFO("设置成功：%s = %d", name.c_str(), value);
            return true;
        } else {
            ROS_WARN("设置失败：%s = %d（错误：%s）", 
                     name.c_str(), value, strerror(errno));
            return false;
        }
    }

    // 初始化相机控制参数（硬件级调节）
    void initCameraControls() {
        // 基础图像参数
        setV4L2Control(0x00980900, brightness_, "亮度");
        setV4L2Control(0x00980901, contrast_, "对比度");
        setV4L2Control(0x00980902, saturation_, "饱和度");
        setV4L2Control(0x0098091b, sharpness_, "锐度");
        setV4L2Control(0x00980913, gain_, "增益");
        setV4L2Control(0x0098091c, backlight_comp_, "背光补偿");

        // 白平衡（先关闭自动再设置档位）
        setV4L2Control(0x0098090c, wb_auto_ ? 1 : 0, "自动白平衡");
        if (!wb_auto_) {
            setV4L2Control(0x0098091a, wb_temp_, "白平衡色温档位");
        }

        // 对焦（先关闭自动再设置值）
        setV4L2Control(0x009a090c, focus_auto_ ? 1 : 0, "自动对焦");
        if (!focus_auto_) {
            setV4L2Control(0x009a090a, focus_abs_, "手动对焦值");
        }
    }

    // 运行节点：读取图像并发布
    void run() {
        // 打开相机（OpenCV读取视频流）
        cv::VideoCapture cap(device_path_);
        if (!cap.isOpened()) {
            ROS_FATAL("OpenCV无法打开相机: %s", device_path_.c_str());
            return;
        }

        // 配置图像格式与参数（优先MJPG）
        cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));  // MJPG格式
        cap.set(cv::CAP_PROP_FRAME_WIDTH, width_);
        cap.set(cv::CAP_PROP_FRAME_HEIGHT, height_);
        cap.set(cv::CAP_PROP_FPS, fps_);

        // 验证实际配置
        verifyCameraConfig(cap);

        // 循环读取并发布图像
        cv::Mat frame;
        ros::Rate rate(publish_rate_);
        while (nh_.ok()) {
            cap >> frame;
            if (frame.empty()) {
                ROS_WARN("未获取到图像（可能分辨率不支持或带宽不足）");
                rate.sleep();
                continue;
            }

            // 转换为ROS图像消息
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(
                std_msgs::Header(), "bgr8", frame
            ).toImageMsg();
            msg->header.stamp = ros::Time::now();
            msg->header.frame_id = frame_id_;
            image_pub_.publish(msg);

            ros::spinOnce();
            rate.sleep();
        }
        cap.release();
    }

    // 验证相机实际配置
    void verifyCameraConfig(cv::VideoCapture& cap) {
        int fourcc = static_cast<int>(cap.get(cv::CAP_PROP_FOURCC));
        char format[5];
        snprintf(format, sizeof(format), "%c%c%c%c", 
                 fourcc & 0xFF, (fourcc >> 8) & 0xFF, 
                 (fourcc >> 16) & 0xFF, (fourcc >> 24) & 0xFF);
        
        int actual_width = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_WIDTH));
        int actual_height = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_HEIGHT));
        int actual_fps = static_cast<int>(cap.get(cv::CAP_PROP_FPS));

        ROS_INFO("实际图像配置：");
        ROS_INFO("  格式: %s", format);
        ROS_INFO("  分辨率: %dx%d", actual_width, actual_height);
        ROS_INFO("  帧率: %d fps", actual_fps);
    }
};


int main(int argc, char**argv) {
    setlocale(LC_CTYPE, "zh_CN.utf8");
    ros::init(argc, argv, "multiple_cameras_node");
    UVCCameraNode node;
    node.run();
    return 0;
}